#include "percepcion.h"

percepcionLR2::percepcionLR2(){
	servoUS.inicializarServo(PIN_SERVO);
	sensoresUS[0].setPin(PIN_US0);
	sensoresUS[1].setPin(PIN_US1);
	sensoresUS[2].setPin(PIN_US2);
	sensoresUS[3].setPin(PIN_US3);
	sensoresUS[4].setPin(PIN_US4);
	sensoresUS[5].setPin(PIN_US5);
	sensoresUS[6].setPin(PIN_US6);
	sensoresUS[7].setPin(PIN_US7);
	sensoresUS[8].setPin(PIN_US8);
	sensoresIR[0].setPin(PIN_IR0);
	sensoresIR[1].setPin(PIN_IR1);
	sensoresIR[2].setPin(PIN_IR2);
	sensoresIR[3].setPin(PIN_IR3);
	sensoresIR[4].setPin(PIN_IR4);
	sensoresIR[5].setPin(PIN_IR5);
}


void percepcionLR2::inicializarSistemaPerecepcion() {
	servoUS.inicializarServo(PIN_SERVO);
	sensoresUS[0].setPin(PIN_US0);
	sensoresUS[1].setPin(PIN_US1);
	sensoresUS[2].setPin(PIN_US2);
	sensoresUS[3].setPin(PIN_US3);
	sensoresUS[4].setPin(PIN_US4);
	sensoresUS[5].setPin(PIN_US5);
	sensoresUS[6].setPin(PIN_US6);
	sensoresUS[7].setPin(PIN_US7);
	sensoresUS[8].setPin(PIN_US8);
	sensoresIR[0].setPin(PIN_IR0);
	sensoresIR[1].setPin(PIN_IR1);
	sensoresIR[2].setPin(PIN_IR2);
	sensoresIR[3].setPin(PIN_IR3);
	sensoresIR[4].setPin(PIN_IR4);
	sensoresIR[5].setPin(PIN_IR5);
}


unsigned short int percepcionLR2::obtenerMedidaUS(int idUS) {
	unsigned short int distancia;
	distancia = filtroUS(idUS);
	return distancia;
}

uint8_t percepcionLR2::obtenerMedidaIR(int idIR) {
	uint8_t distancia;
	distancia = filtroIR(idIR);
	return distancia;
}

unsigned short int  percepcionLR2::obtenerMedidaUSTrasero(int angulo) {
	unsigned short int  distancia;
	if(angulo > 130){ angulo = 130; }else{ if(angulo < 50){ angulo = 50; } }
	servoUS.escribirServomotor(angulo);
	delay(200);
	distancia = obtenerMedidaUS(8);
	return distancia;
}

unsigned short int* percepcionLR2::obtenerBarridoFrontalUS() {
	unsigned short int distancias[6];
	for (int i = 0; i < 3; ++i) {
		distancias[i] = obtenerMedidaUS( i + 1 );
		distancias[i + 3] = obtenerMedidaUS( i + 4 );
	}
	return distancias;
}

uint8_t* percepcionLR2::obtenerBarridoFrontalIR() {
	uint8_t distancias[6];
	for (int i = 0; i < 3; ++i) {
		distancias[i] = obtenerMedidaIR( i + 1 );
		distancias[i + 3] = obtenerMedidaIR( i + 4 );
	}
	return distancias;
}

unsigned short int * percepcionLR2::obtenerBarridoTraseroUS() {
	unsigned short int distancias[5];
	int angulo = 50;
    servoUS.escribirServomotor(angulo);
	delay(200);
	for (int i = 0; i < 5; ++i) {
		distancias[i] = obtenerMedidaUS(8);
		angulo = angulo + 20;
		servoUS.escribirServomotor(angulo);
		delay(45);
	}
	servoUS.escribirServomotor(90);
	return distancias;
}

percepcionLR2::~percepcionLR2() {
	servoUS.~servoMotor();
	for (int var = 0; var < 8; ++var) {
		sensoresUS[var].~sensorUS();
	}
	for (int var = 0; var < 5; ++var) {
		sensoresIR[var].~sensorIR();
	}
}

unsigned short int percepcionLR2::filtroUS(int idUS) {
	int vector[SIZE_FILTRO_US], mediana, medidaPulgadas, medidaCm;
	for( int i = 0 ; i < SIZE_FILTRO_US ; i++ ){
		vector[i] = sensoresUS[idUS].obtenerLectura();
		delay(TIEMPO_MUESTREO_US);
	}
	for( int i = 0 ; i < SIZE_FILTRO_US ; i++ ){
		int j = vector[i], k;
		for( k = i - 1 ; (k >= 0) && (j < vector[k]) ; k-- ){
			vector[ k + 1 ] = vector[ k ];
		}
		vector[ k + 1 ] = j;
	}
	mediana = (int)(SIZE_FILTRO_US/2);
	medidaPulgadas = (int)(vector[mediana]/2);
	medidaCm = (unsigned short int)( medidaPulgadas * 2.54 );
	return medidaCm;
}

uint8_t percepcionLR2::filtroIR(int idIR) {
	    float voltios, distancia;
	    int mediana;
	    float vector[SIZE_FILTRO_IR];
	    for( int i = 0 ; i < SIZE_FILTRO_IR ; i++){
	        vector[i] = sensoresIR[idIR].obtenerLectura();
	        delay(TIEMPO_MUESTREO_IR);
	    }
	    for( int i = 0 ; i < SIZE_FILTRO_IR ; i++ ){
	        int j = vector[i], k;
	        for( k = i - 1 ; (k >= 0) && (j < vector[k]) ; k-- ){
	            vector[ k + 1 ] = vector[ k ];
	        }
	        vector[ k + 1 ] = j;
	    }
	    mediana = (int)(SIZE_FILTRO_IR/2);
	    voltios = vector[mediana] * RESOLUCION_IR;
	    if( (voltios >= 0.4) && (voltios < 2.7) ){
	        distancia=((16.75*pow(voltios,4.0))-(119.26*pow(voltios,3.0))+(311.7*pow(voltios,2.0))-(365.71*voltios)+184.03);
	    }else{
	    	if(voltios < 0.4){
	    		distancia = 0;
	    	}else{
	    		distancia = 80;
	    	}
	    }
	    return (uint8_t)distancia;
}
